/**
 * @author Yunfeng Bai
 *
 * 2010-11-27
 */
package force;

import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import collision.Collision;

import rigidbody.RigidBody;
import sun.security.util.PendingException;
import util.Util;

public class SpringPenaltyForce extends Force
{
    public static final double K = 100;

    public RigidBody _body1, _body2;

    protected double _startDist;
    
    protected Point3d _pointInObj1, _pointInObj2;
    
    /** @see Collision#type */
    public int _type;
    
    protected double _penetrationDepth;
    
    protected Vector3d _contactNormal = new Vector3d();
    
    /**
     * Designate the mode of the spring.
     * mode == 0: spring between two centers of RigidBody's.
     * mode == 1: spring between two contact points of RigidBody's
     */
    protected int _intersectionMode;

    public SpringPenaltyForce(RigidBody b1_, RigidBody b2_)
    {
        _body1 = b1_;
        _body2 = b2_;
        _startDist = Util.distance(b1_.getPosition(), b2_.getPosition());
        _intersectionMode = 0;
    }
    
    public SpringPenaltyForce(RigidBody b1_, RigidBody b2_, Point3d pointInWorldSpace_)
    {
    	_type = 0;
    	Matrix4d invTrans1 = (Matrix4d) b1_.getTransformMatrix().clone();
    	invTrans1.invert();
    	Matrix4d invTrans2 = (Matrix4d) b2_.getTransformMatrix().clone();
    	invTrans2.invert();
    	Point3d p1 = (Point3d)pointInWorldSpace_.clone();
    	Point3d p2 = (Point3d)pointInWorldSpace_.clone();
    	invTrans1.transform(p1);
    	invTrans2.transform(p2);
    	_body1 = b1_;
        _body2 = b2_;
        _startDist = 0;
        _pointInObj1 = p1;
        _pointInObj2 = p2;
        _intersectionMode = 1;
    }
    
    public SpringPenaltyForce(RigidBody b1_, RigidBody b2_, Point3d pointInWorldSpace_, double penetrationDepth,
    						Vector3d contactNormal)
    {
    	_type = 1;
    	_body1 = b1_;
        _body2 = b2_;
        _pointInObj1 = pointInWorldSpace_;
        _penetrationDepth = penetrationDepth;
        _contactNormal.set(contactNormal);
        if (Double.isNaN(contactNormal.x) || Double.isNaN(contactNormal.y) || Double.isNaN(contactNormal.z))
        {
        	throw new RuntimeException();
        }
        if (_contactNormal.lengthSquared() != 0)
        	_contactNormal.normalize();
    }
    
    private double getDistance(Point3d p1, Point3d p2)
    {
    	p1 = (Point3d)_pointInObj1.clone();
    	p2 = (Point3d)_pointInObj2.clone();
    	_body1.getTransformMatrix().transform(p1);
    	_body2.getTransformMatrix().transform(p2);
    	return Util.distance(p1, p2);
    }
    
    @Override
    public void apply(double t)
    {
    	if (_type == 0)
    	{
    		applyType0();
    	}
    	else
    	{
    		applyType1();
    	}
    }

	private void applyType1()
	{
		_contactNormal.scale(_penetrationDepth * K);
		if (_body1.isImmobile())
		{
			_body1.accumulateForce(_contactNormal);
	        _body1.accumulateTorque(_pointInObj1, _contactNormal);
		}
        _contactNormal.scale(-1);
        if (_body2.isImmobile())
        {
	        _body2.accumulateForce(_contactNormal);
	        _body2.accumulateTorque(_pointInObj1, _contactNormal);
        }
	}

	private void applyType0() {
		Vector3d b2Tob1 = new Vector3d();
        b2Tob1.set(_body1.getPosition());
        b2Tob1.sub(_body2.getPosition());
        b2Tob1.normalize();
        double distance = 0;
        Point3d p1, p2;
        if (_intersectionMode == 0)
        {
        	distance = Util.distance(_body1.getPosition(), _body2.getPosition());
        	p1 = _body1.getPosition();
        	p2 = _body2.getPosition();
        }
        else
        {
        	p1 = new Point3d();
        	p2 = new Point3d();
        	distance = getDistance(p1, p2);
        }
        
//        if (distance > _startDist)
//            return;
        b2Tob1.scale(distance);

        Vector3d b1Tob2 = new Vector3d();
        b1Tob2.set(b2Tob1);
        b1Tob2.scale(-1);

        b2Tob1.scale(K);
        b1Tob2.scale(K);

        if (_body1.isImmobile())
        {
	        _body1.accumulateForce(b2Tob1);
	        _body1.accumulateTorque(p1, b2Tob1);
        }
        if (_body2.isImmobile())
        {
	        _body2.accumulateForce(b1Tob2);
	        _body2.accumulateTorque(p2, b1Tob2);
        }
	}

    @Override
    public int hashCode()
    {
        final int prime = 31;
        int result = 1;
        if (_body1 == null || _body2 == null)
            throw new RuntimeException("b1 or b2 should not be null");
        int h1 = _body1.hashCode();
        int h2 = _body2.hashCode();
        if (h1 > h2)
        {
            int t = h1;
            h1 = h2;
            h2 = t;
        }
        result = prime * result + h1;
        result = prime * result + h2;
        return result;
    }

    @Override
    public boolean equals(Object obj)
    {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        SpringPenaltyForce other = (SpringPenaltyForce) obj;
        if (_body1 == null || _body2 == null || other._body1 == null || other._body2 == null)
            throw new RuntimeException("b1 or b2 should not be null");
        if ((_body1.equals(other._body1) && _body2.equals(other._body2))
                || (_body1.equals(other._body2) && _body2.equals(other._body1)))
        {
            return true;
        }
        return false;
    }
}
